#include "FCHD1114.h"


// 读取单个传感器的函数
uint16_t readGraySensor(int sensorPin) {
    uint16_t value = digitalRead(sensorPin);
    return value;
}

// 读取传感器数组的函数
uint16_t readGraySensorArray(const int* sensorPins) {
    uint16_t temp = 0;
    for (int i = 0; i < NUM_SENSORS; i++) {
        temp |= (readGraySensor(sensorPins[i])<< i);
    }
    return temp;    // 返回平均值
}

// 获取寻迹偏差
float getError(uint16_t grayValue, int scale) {
    int R,L;
    float error;
    switch (grayValue)
    {
    case 0x01:
        L=2;R=0;error=(R-L)*scale;break;
    case 0x02:
        L=1;R=0;error=(R-L)*scale;break;    
    case 0x04:
        L=0;R=0;error=(R-L)*scale;break;  
    case 0x08:
        L=0;R=1;error=(R-L)*scale;break;  
    case 0x10:
        L=0;R=2;error=(R-L)*scale;break;  

    default:
        error=10000; break;
    }
    return error;
}
